﻿#region Using Directive

using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.Collections.Generic;

#endregion

namespace ParticleTracker
{
    public class ParticleTrackingVelocimetry : ParticleTrackingMethod
    {
        public const int R1 = 5;
        public const int R2 = 3;
        int r1 = R1, r2 = R2;
        static uint PARTICLE_SIZE_MIN =  50;
        public ParticleTrackingVelocimetry(ParticleReel reel) : base(reel)
        {

        }
        public ParticleTrackingVelocimetry(ParticleReel reel, int r1, int r2)
            : base(reel)
        {
            this.r1 = r1;
            this.r2 = r2;
        }
        private Particle AddPointToParticle(ParticleFrame frame, int x, int y, int newX, int newY)
        {
            string pos = string.Format("{0},{1}", x, y);
            string newPos = string.Format("{0},{1}", newX, newY);
            Particle p = null;
            try
            {
                p = (Particle)frame.PieceMap[newPos];
                p.locations.Add(new Point(x, y));
                frame.PieceMap[pos] = p;
            }
            catch (KeyNotFoundException)
            {
                //p = new Particle();
                //p.locations.Add(new Point(x, y));
                //frame.Particles.Add(p);
                //frame.ParticleMap[pos] = p;
            }
            return p;
        }
        private void FindParticles()
        {
            foreach (ParticleFrame frame in reel.Frames)
            {
                Bitmap img = frame.OriginalImage;
                for (int y = 0; y < img.Height; y++)
                {
                    for (int x = 0; x < img.Width; x++)
                    {
                        Color c = img.GetPixel(x, y);
                        if (c.R != Color.White.R) continue;
                        Color s;
                        Particle added = null;

                        if (x > 0)
                        {
                            s = img.GetPixel(x - 1, y);
                            if (s.R == Color.White.R)
                                added = this.AddPointToParticle(frame, x, y, x - 1, y);
                            if (added != null) continue;
                            if (y > 0)
                            {
                                s = img.GetPixel(x - 1, y - 1);
                                if (s.R == Color.White.R)
                                    added = this.AddPointToParticle(frame, x, y, x - 1, y - 1);
                                if (added != null) continue;
                            }
                        }
                        if (y > 0)
                        {
                            s = img.GetPixel(x, y - 1);
                            if (s.R == Color.White.R)
                                added = this.AddPointToParticle(frame, x, y, x, y - 1);
                            if (added != null) continue;
                            if (x < img.Width - 1)
                            {
                                s = img.GetPixel(x + 1, y - 1);
                                if (s.R == Color.White.R)
                                    added = this.AddPointToParticle(frame, x, y, x + 1, y - 1);
                                if (added != null) continue;
                            }
                        }

                        {
                            Particle particle = new Particle();
                            particle.locations.Add(new Point(x, y));
                            frame.Pieces.Add(particle);
                            frame.PieceMap[string.Format("{0},{1}", x, y)] = particle;
                        }
                    }
                }
                frame.Pieces.RemoveAll(x => ((Particle)x).locations.Count < PARTICLE_SIZE_MIN);
            }
        }
        private void CenterParticles()
        {
            foreach (ParticleFrame f in reel.Frames)
            {
                foreach (Particle p in f.Pieces)
                {
                    Point maxPoint = new Point(Int32.MinValue, Int32.MinValue);
                    Point minPoint = new Point(Int32.MaxValue, Int32.MaxValue);
                    foreach (Point x in p.locations)
                    {
                        if (x.X > maxPoint.X)
                            maxPoint.X = x.X;
                        if (x.X < minPoint.X)
                            minPoint.X = x.X;
                        if (x.Y > maxPoint.Y)
                            maxPoint.Y = x.Y;
                        if (x.Y < minPoint.Y)
                            minPoint.Y = x.Y;
                    }
                    p.Center = new Point(
                        (int)((maxPoint.X + minPoint.X) / 2),
                        (int)((maxPoint.Y + minPoint.Y) / 2)
                        );
                }
            }
        }
        private void TrackParticles(int i)
        {
            ParticleFrame current = reel.Frames[i];
            ParticleFrame next = reel.Frames[i + 1];
            ParticleFrame future = i < reel.Frames.Count - 2 ? reel.Frames[i + 2] : null;
            ParticleFrame prev = i > 0 ? reel.Frames[i - 1] : null;
            foreach (Particle p in current.Pieces)
            {
                List<Particle> possibles = new List<Particle>();
                List<Point> trajectories = new List<Point>();
                // Find possibles in i+1
                for (int x = p.Center.X - r1; x < p.Center.X + r1; x++)
                {
                    for (int y = p.Center.Y - r1; y < p.Center.Y + r1; y++)
                    {
                        string pos = string.Format("{0},{1}", x, y);
                        if (!next.PieceMap.ContainsKey(pos)) continue;
                        Particle test = (Particle)next.PieceMap[pos];
                        if (possibles.IndexOf(test) >= 0) continue;
                        possibles.Add(test);
                        trajectories.Add(
                            new Point(
                                test.Center.X - p.Center.X,
                                test.Center.Y - p.Center.Y)
                        );
                    }
                }
                // Try to confirm possibles. in i+2
                for (int k = 0; future != null && k < possibles.Count; k++)
                {
                    Point pt = possibles[k].Center;
                    Point newPt = new Point(
                        pt.X + trajectories[k].X,
                        pt.Y + trajectories[k].Y
                        );
                    for (int x = newPt.X - r2; x < newPt.X + r2; x++)
                    {
                        for (int y = newPt.Y - r2; y < newPt.Y + r2; y++)
                        {
                            string pos = string.Format("{0},{1}", x, y);
                            if (!future.PieceMap.ContainsKey(pos)) continue;
                            Particle test = (Particle)future.PieceMap[pos];
                            p.velocity.v.X = trajectories[k].X;
                            p.velocity.v.Y = trajectories[k].Y;
                            p.velocity.confidence = 0.75;
                        }
                    }
                }
                // Try to confirm possibles. in i-1
                for (int k = 0; prev != null && k < possibles.Count; k++)
                {
                    Point newPt = new Point(
                        p.Center.X - trajectories[k].X,
                        p.Center.Y - trajectories[k].Y
                        );
                    for (int x = newPt.X - R2; x < newPt.X + R2; x++)
                    {
                        for (int y = newPt.Y - R2; y < newPt.Y + R2; y++)
                        {
                            string pos = string.Format("{0},{1}", x, y);
                            if (!prev.PieceMap.ContainsKey(pos)) continue;
                            Particle test = (Particle)prev.PieceMap[pos];
                            if (p.velocity.v.X == 0 && p.velocity.v.Y == 0)
                            {
                                p.velocity.v.X = trajectories[k].X;
                                p.velocity.v.Y = trajectories[k].Y;
                                p.velocity.isSuspect = true;
                                p.velocity.confidence = 0.75;
                            }
                            else
                            {
                                p.velocity.confidence = 1.0;
                            }
                        }
                    }
                }
            }
        }
        public override void Run()
        {
            this.FindParticles();
            this.CenterParticles();
            for (int i = 0; i < reel.Frames.Count - 1; i++)
            {
                this.TrackParticles(i);
            }
        }
    }
}
